#!/usr/bin/env python3

import json
import time
import os
import requests
import subprocess
import re
import threading
import tempfile
import gc
import signal
import sys
import atexit
import logging
import serial
from datetime import datetime
from concurrent.futures import ThreadPoolExecutor, TimeoutError
from vosk import Model, KaldiRecognizer

class RoboticArmController:
    def __init__(self):
        # 设置工作目录
        self.working_dir = os.path.dirname(os.path.abspath(__file__))
        os.chdir(self.working_dir)
        
        # 初始化日志
        self.setup_logging()
        
        self.logger.info("🚀 启动机械臂语音控制程序...")
        print("🤖 正在初始化机械臂语音控制系统...")
        
        # 初始化串口通信
        self.serial_connection = None
        self.setup_serial_communication()
        
        # 模型路径检查
        model_path = os.path.join(self.working_dir, "vosk-model-small-cn-0.22")
        if not os.path.exists(model_path):
            model_path = "vosk-model-small-cn-0.22"
            print("🔍 正在查找语音模型文件...")
        
        try:
            print("📚 正在加载语音识别模型...")
            self.model = Model(model_path)
            self.logger.info(f"✅ 语音模型加载成功: {model_path}")
            print("✅ 语音识别模型加载完成")
        except Exception as e:
            self.logger.error(f"❌ 语音模型加载失败: {e}")
            print(f"❌ 语音模型加载失败: {e}")
            self.model = None
        
        self.api_key = "ms-fecb8eca-0503-44e9-9cf2-4870bb0ed37c"
        self.base_url = "https://api-inference.modelscope.cn/v1"
        self.model_name = "deepseek-ai/DeepSeek-V3.2-Exp"
        self.speaking = False
        
        # USB 音频设备配置
        self.usb_speaker_device = "hw:CARD=Y02,DEV=0"
        self.usb_mic_device = "hw:CARD=Y02,DEV=0"
        
        # 机械臂状态
        self.arm_state = {
            "base": 500, "shoulder": 500, "elbow": 500, 
            "wrist_vert": 500, "wrist_rot": 500, "gripper": 0
        }
        
        self.servo_ids = {
            "base": 1, "shoulder": 2, "elbow": 3, 
            "wrist_vert": 4, "wrist_rot": 5, "gripper": 6
        }
        
        # 程序状态
        self.running = True
        self.sleep_mode = False
        self.last_activity_time = time.time()
        self.idle_timeout = 60  # 1分钟无操作进入休眠（测试用）
        
        # 唤醒参数 - 基于实时监听
        self.wake_threshold = 0.03  # 提高音量阈值
        self.ambient_noise_level = 0.005  # 环境噪音水平
        self.wake_duration = 0.2  # 持续唤醒时间(秒)
        self.debug_mode = True  # 启用调试模式
        
        # 性能优化
        self.command_cache = {}
        self.cache_size = 8
        self.api_executor = ThreadPoolExecutor(max_workers=1)
        self.last_api_success = True
        
        # 预加载识别器
        if self.model:
            print("🎯 初始化语音识别器...")
            self.recognizer = KaldiRecognizer(self.model, 16000)
            print("✅ 语音识别器初始化完成")
        else:
            self.recognizer = None
            print("⚠️ 语音识别器未初始化")
            
        # 录音参数
        self.silence_timeout = 2.0
        self.max_record_time = 8.0
        self.voice_threshold = 0.015
        
        # 监听状态
        self.listening = False
        self.wake_listening = False
        
        # 调试统计
        self.wake_attempts = 0
        self.successful_wakes = 0
        self.volume_samples = []
        
        # 注册信号处理
        signal.signal(signal.SIGINT, self.signal_handler)
        signal.signal(signal.SIGTERM, self.signal_handler)
        atexit.register(self.cleanup)
        
        self.logger.info("🤖 机械臂语音控制程序初始化完成")
        print("🎉 机械臂语音控制系统初始化完成！")
        print("💡 系统提示：")
        print("   - 说 '1' 或 '语音控制' 进入语音控制模式")
        print("   - 说 '2' 或 '复位' 将机械臂复位到初始位置")
        print("   - 说 '3' 或 '状态' 查看当前机械臂状态")
        print("   - 说 '4' 或 '休眠' 进入休眠模式")
        print("   - 在休眠模式下，大声说话或拍手可以唤醒系统")
        
        # 测试音频设备
        self.test_audio_devices()
        
        # 播放启动语音
        self.play_voice("system_start")
        
        # 启动监控线程
        self.sleep_monitor_thread = threading.Thread(target=self.monitor_sleep, daemon=True)
        self.sleep_monitor_thread.start()
        print("🔍 休眠监控线程已启动")
        
        # 启动调试监控线程
        if self.debug_mode:
            self.debug_monitor_thread = threading.Thread(target=self.debug_monitor, daemon=True)
            self.debug_monitor_thread.start()
            print("🐛 调试监控线程已启动")
    
    def setup_serial_communication(self):
        """使用 pyserial 初始化串口通信"""
        print("🔌 使用 pyserial 初始化串口通信...")
        try:
            # 使用 pyserial 打开串口
            self.serial_connection = serial.Serial(
                port='/dev/ttySTM1',
                baudrate=115200,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                timeout=1,
                write_timeout=1,
                xonxoff=False,
                rtscts=False,
                dsrdtr=False
            )
            
            # 等待串口初始化
            time.sleep(2)
            
            if self.serial_connection.is_open:
                print("✅ pyserial 串口通信初始化成功: /dev/ttySTM1, 115200bps")
                self.logger.info("✅ pyserial 串口通信初始化成功: /dev/ttySTM1, 115200bps")
            else:
                print("❌ 串口打开失败")
                self.logger.error("❌ 串口打开失败")
                self.serial_connection = None
                
        except Exception as e:
            print(f"❌ pyserial 串口初始化失败: {e}")
            self.logger.error(f"❌ pyserial 串口初始化失败: {e}")
            self.serial_connection = None
    
    def send_serial_command(self, command, description=""):
        """使用 pyserial 通过串口发送命令到电脑"""
        if not self.serial_connection or not self.serial_connection.is_open:
            if description:
                self.logger.warning(f"❌ 串口未连接，无法发送: {description}")
            return False
        
        try:
            # 添加换行符并编码
            command_str = f"{command}\n"
            encoded_command = command_str.encode('utf-8')
            
            # 使用 pyserial 发送命令
            bytes_written = self.serial_connection.write(encoded_command)
            self.serial_connection.flush()
            
            if description:
                self.logger.info(f"[串口发送] {description}: {command}")
            
            return bytes_written > 0
            
        except Exception as e:
            self.logger.error(f"❌ pyserial 串口发送失败 {description}: {e}")
            return False
    
    def send_command_to_arm(self, command):
        """发送命令到机械臂控制器 - 通过 pyserial 串口发送"""
        self.logger.info(f"[发送命令] {command}")
        print(f"🔄 发送命令到机械臂: {command}")
        
        # 通过 pyserial 串口发送命令到电脑
        success = self.send_serial_command(command, f"机械臂控制指令")
        
        if success:
            self.logger.info(f"✅ 命令已通过 pyserial 串口发送: {command}")
            print(f"✅ 命令已通过 pyserial 串口发送: {command}")
        else:
            self.logger.error(f"❌ 命令发送失败: {command}")
            print(f"❌ 命令发送失败: {command}")
    
    def send_movemulti_command(self, positions):
        """使用 pyserial 发送 movemulti 命令到机械臂"""
        # 构建 movemulti 命令格式: movemulti 1,500,2,500,3,500,4,500,5,500,6,0
        command_parts = []
        for i, pos in enumerate(positions, 1):
            command_parts.append(str(i))  # 舵机ID
            command_parts.append(str(pos))  # 位置
        
        command = f"movemulti {','.join(command_parts)}"
        self.send_command_to_arm(command)
        return command

    def execute_commands_optimized(self, command_data):
        """优化命令执行 - 使用 pyserial 通过串口发送"""
        if "error" in command_data:
            error_msg = f"错误: {command_data['error']}"
            self.play_voice("command_parse_failed")
            self.logger.error(f"❌ {error_msg}")
            print(f"❌ {error_msg}")
            return False
        
        action = command_data.get('action', '未知动作')
        duration = command_data.get('duration', 2)
        
        self.update_activity_time()
        
        self.play_voice("execution_start")
        self.logger.info(f"🎯 执行动作: {action}")
        print(f"🎯 执行动作: {action}")
        
        commands = command_data.get("commands", [])
        
        if commands:
            command_details = []
            for cmd in commands:
                servo = cmd.get("servo", "")
                position = cmd.get("position", 0)
                if servo:
                    command_details.append(f"{servo}到{position}位置")
            
            if command_details:
                detail_text = "，".join(command_details)
                print(f"🔧 具体动作: {detail_text}")
                self.speak_direct(f"具体动作：{detail_text}")
        
        # 构建位置数组，按照舵机ID顺序
        positions = []
        position_map = {}  # 用于记录实际设置的位置
        
        # 首先设置所有舵机为当前状态
        for servo_name in ['base', 'shoulder', 'elbow', 'wrist_vert', 'wrist_rot', 'gripper']:
            target_pos = self.arm_state[servo_name]  # 默认为当前状态
            
            # 检查是否有新的目标位置
            for cmd in commands:
                if cmd.get("servo") == servo_name:
                    target_pos = cmd.get("position")
                    break
            
            positions.append(target_pos)
            position_map[servo_name] = target_pos
            self.arm_state[servo_name] = target_pos
        
        # 使用 pyserial 发送 movemulti 命令
        self.play_voice("sending_command")
        sent_command = self.send_movemulti_command(positions)
        self.logger.info(f"[硬件控制] {sent_command}")
        print(f"🔄 发送控制指令: {sent_command}")
        
        def _wait_and_notify():
            time.sleep(duration)
            self.play_voice("execution_complete")
            print("✅ 动作执行完成")
        
        threading.Thread(target=_wait_and_notify, daemon=True).start()
        
        self.play_voice("execution_progress")
        self.logger.info("⏳ 执行中...")
        print(f"⏳ 执行中...", end="", flush=True)
        for i in range(min(3, duration)):
            time.sleep(1)
            print(".", end="", flush=True)
        print(" 完成!")
        
        self.play_voice("status_updated")
        self.logger.info(f"📍 更新后状态: {self.arm_state}")
        print(f"📍 机械臂状态已更新: {self.arm_state}")
        return True

    def reset_arm_optimized(self):
        """优化复位功能 - 使用 pyserial 通过串口发送复位命令"""
        if self.sleep_mode:
            return
            
        self.play_voice("enter_reset_mode")
        self.logger.info("🔄 复位机械臂")
        print("\n🔄 复位机械臂")
        
        # 构建复位位置数组
        reset_positions = [500, 500, 500, 500, 500, 0]  # 对应6个舵机的复位位置
        
        # 使用 pyserial 发送复位命令
        self.send_movemulti_command(reset_positions)
        
        # 更新状态
        self.arm_state = {
            "base": 500, "shoulder": 500, "elbow": 500, 
            "wrist_vert": 500, "wrist_rot": 500, "gripper": 0
        }
        
        self.logger.info("✅ 机械臂复位完成")
        print("✅ 机械臂复位完成")

    def test_audio_devices(self):
        """测试音频设备"""
        print("🔊 测试音频设备...")
        self.logger.info("🔊 测试音频设备...")
        
        try:
            # 测试麦克风
            print("🎤 测试麦克风...")
            test_file = "/tmp/mic_test.wav"
            result = subprocess.run([
                "arecord", "-D", self.usb_mic_device,
                "-f", "S16_LE", "-r", "44100", "-c", "2", "-d", "1",
                test_file
            ], capture_output=True, timeout=5)
            
            if result.returncode == 0 and os.path.exists(test_file):
                file_size = os.path.getsize(test_file)
                if file_size > 5000:
                    print("✅ 麦克风测试通过")
                    self.logger.info("✅ 麦克风测试通过")
                else:
                    print("⚠️ 麦克风录音文件较小")
                    self.logger.warning("⚠️ 麦克风录音文件较小")
                os.unlink(test_file)
            else:
                print("⚠️ 麦克风测试失败，但继续运行")
                self.logger.warning("⚠️ 麦克风测试失败")
                
            # 测试扬声器
            print("🔊 测试扬声器...")
            test_sound = subprocess.run([
                "aplay", "-D", self.usb_speaker_device,
                "-q"
            ], input=b"", capture_output=True, timeout=3)
            
            if test_sound.returncode == 0:
                print("✅ 扬声器测试通过")
                self.logger.info("✅ 扬声器测试通过")
            else:
                print("⚠️ 扬声器测试失败，但继续运行")
                self.logger.warning("⚠️ 扬声器测试失败")
                
        except Exception as e:
            print(f"⚠️ 音频设备测试异常: {e}")
            self.logger.warning(f"⚠️ 音频设备测试异常: {e}")
    
    def debug_monitor(self):
        """调试监控线程 - 定期显示系统状态"""
        while self.running:
            if self.sleep_mode:
                # 在休眠模式下显示详细状态
                current_time = time.time()
                idle_time = current_time - self.last_activity_time
                
                print("\n" + "="*60)
                print("🔍 系统调试信息 - 休眠状态")
                print("="*60)
                print(f"⏰ 休眠时间: {idle_time:.1f}秒")
                print(f"📊 唤醒阈值: {self.wake_threshold:.4f}")
                print(f"🔊 环境噪音: {self.ambient_noise_level:.4f}")
                print(f"⏱️ 唤醒持续时间: {self.wake_duration:.1f}秒")
                print(f"🔄 唤醒尝试: {self.wake_attempts}次")
                print(f"✅ 成功唤醒: {self.successful_wakes}次")
                
                if self.volume_samples:
                    avg_volume = sum(self.volume_samples) / len(self.volume_samples)
                    max_volume = max(self.volume_samples)
                    print(f"📈 平均音量: {avg_volume:.4f}")
                    print(f"📊 最大音量: {max_volume:.4f}")
                    print(f"📋 样本数量: {len(self.volume_samples)}")
                
                # 显示唤醒成功率
                if self.wake_attempts > 0:
                    success_rate = (self.successful_wakes / self.wake_attempts) * 100
                    print(f"🎯 唤醒成功率: {success_rate:.1f}%")
                
                print("\n💡 调试提示:")
                print("  🔊 正常说话音量应该在 0.02-0.05 之间")
                print("  👏 拍手音量通常在 0.05-0.15 之间")
                print(f"  ⏱️ 需要持续 {self.wake_duration}秒 超过阈值的声音")
                print("  🎯 当前使用44.1kHz立体声高质量监听")
                print("="*60)
                
            time.sleep(30)  # 每30秒显示一次调试信息
    
    def setup_logging(self):
        """设置日志系统"""
        print("📝 正在初始化日志系统...")
        log_dir = os.path.join(self.working_dir, "logs")
        os.makedirs(log_dir, exist_ok=True)
        
        log_file = os.path.join(log_dir, f"robotic_arm_{datetime.now().strftime('%Y%m%d')}.log")
        
        logging.basicConfig(
            level=logging.INFO,
            format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
            handlers=[
                logging.FileHandler(log_file),
                logging.StreamHandler(sys.stdout)
            ]
        )
        self.logger = logging.getLogger("RoboticArmController")
        print(f"✅ 日志系统初始化完成，日志文件: {log_file}")
    
    def signal_handler(self, signum, frame):
        """信号处理"""
        print(f"\n📢 收到关闭信号 {signum}，正在安全关闭系统...")
        self.logger.info(f"📢 收到信号 {signum}，正在关闭...")
        self.running = False
        self.cleanup()
        sys.exit(0)
    
    def cleanup(self):
        """清理资源"""
        print("\n🧹 正在清理系统资源...")
        self.logger.info("🧹 清理资源...")
        self.running = False
        
        # 关闭 pyserial 串口连接
        if self.serial_connection and self.serial_connection.is_open:
            print("🔌 关闭 pyserial 串口连接...")
            try:
                self.serial_connection.close()
                print("✅ pyserial 串口连接已关闭")
            except Exception as e:
                print(f"⚠️ 关闭 pyserial 串口时出错: {e}")
        
        # 关闭API执行器
        if hasattr(self, 'api_executor'):
            print("🔧 关闭API执行器...")
            try:
                self.api_executor.shutdown(wait=False)
            except Exception as e:
                print(f"⚠️ 关闭API执行器时出错: {e}")
        
        # 清理内存
        self._cleanup_memory()
        
        # 输出最终调试统计
        if self.debug_mode:
            print("\n📊 系统运行统计:")
            print(f"  总唤醒尝试: {self.wake_attempts}次")
            print(f"  成功唤醒: {self.successful_wakes}次")
            if self.wake_attempts > 0:
                success_rate = (self.successful_wakes / self.wake_attempts) * 100
                print(f"  唤醒成功率: {success_rate:.1f}%")
        
        print("✅ 资源清理完成，系统安全关闭")
        self.logger.info("✅ 资源清理完成")
    
    def _cleanup_memory(self):
        """清理内存"""
        try:
            print("🧠 清理内存资源...")
            if hasattr(self, 'recognizer'):
                self.recognizer.Reset()
            gc.collect()
            print("✅ 内存清理完成")
        except Exception as e:
            print(f"⚠️ 内存清理警告: {e}")
            self.logger.warning(f"内存清理警告: {e}")
    
    def monitor_sleep(self):
        """监控休眠状态的线程"""
        print("🔍 休眠监控线程运行中...")
        while self.running:
            if not self.sleep_mode:
                idle_time = time.time() - self.last_activity_time
                if idle_time >= self.idle_timeout:
                    print(f"⏰ 系统空闲 {idle_time:.1f}秒，即将进入休眠模式...")
                    self.logger.info(f"⏰ 空闲 {idle_time:.1f}秒，进入休眠模式")
                    self.enter_sleep_mode()
            time.sleep(10)
    
    def realtime_wake_listener(self):
        """实时唤醒监听 - 44.1kHz立体声高质量版本"""
        print("🎯 启动高质量实时唤醒监听系统...")
        self.logger.info("🎯 启动44.1kHz立体声唤醒监听...")
        
        try:
            print("🔧 初始化44.1kHz立体声音频录制设备...")
            cmd = [
                "arecord", "-D", self.usb_mic_device,
                "-f", "S16_LE", "-r", "44100", "-c", "2",  # 44.1kHz 立体声
                "-t", "raw"
            ]
            
            process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
            print("✅ 44.1kHz立体声音频录制设备初始化完成")
            
            loud_start_time = None
            chunk_count = 0
            last_debug_time = time.time()
            
            print("👂 高质量实时监听已启动，请大声说话或拍手唤醒系统...")
            print("💡 使用44.1kHz立体声高质量音频流")
            
            while self.sleep_mode and self.running:
                # 读取0.1秒的数据 (44100Hz * 2声道 * 2字节 * 0.1秒 = 17640字节)
                chunk = process.stdout.read(17640)
                if not chunk:
                    continue
                
                chunk_count += 1
                volume = self.calculate_volume_level_44100_stereo(chunk)
                
                # 记录音量样本用于调试
                if self.debug_mode and len(self.volume_samples) < 100:
                    self.volume_samples.append(volume)
                
                # 每5秒显示一次调试信息
                current_time = time.time()
                if current_time - last_debug_time >= 5:
                    print(f"🔊 当前音量: {volume:.4f}, 阈值: {self.wake_threshold:.4f}")
                    last_debug_time = current_time
                
                # 检查是否超过唤醒阈值
                if volume > self.wake_threshold:
                    if loud_start_time is None:
                        loud_start_time = time.time()
                        print(f"📢 检测到大声量: {volume:.4f}，开始计时...")
                    else:
                        loud_duration = time.time() - loud_start_time
                        progress = min(100, int((loud_duration / self.wake_duration) * 100))
                        print(f"⏱️ 唤醒进度: {progress}% ({loud_duration:.1f}s/{self.wake_duration}s)")
                        
                        if loud_duration >= self.wake_duration:
                            print(f"🎯 唤醒条件达成! 音量: {volume:.4f}, 持续时间: {loud_duration:.1f}s")
                            self.wake_attempts += 1
                            self.successful_wakes += 1
                            process.terminate()
                            process.wait()
                            self.wake_up()
                            return
                else:
                    # 音量低于阈值，重置计时
                    if loud_start_time is not None:
                        duration = time.time() - loud_start_time
                        print(f"⏹️ 音量下降，重置计时 (持续了 {duration:.1f}s)")
                        loud_start_time = None
                        self.wake_attempts += 1
            
            # 退出时清理进程
            if process.poll() is None:
                print("🔧 清理音频录制进程...")
                process.terminate()
                process.wait()
                
        except Exception as e:
            print(f"❌ 实时唤醒监听错误: {e}")
            self.logger.error(f"实时唤醒监听错误: {e}")
    
    def calculate_volume_level_44100_stereo(self, audio_data):
        """计算44.1kHz立体声音频数据的音量水平"""
        try:
            import numpy as np
            if len(audio_data) == 0:
                return 0.0
                
            # 确保数据长度是4的倍数（16位立体声，每个样本4字节）
            if len(audio_data) % 4 != 0:
                audio_data = audio_data[:-(len(audio_data) % 4)]
                
            if len(audio_data) < 4:  # 至少需要4字节（一个立体声样本）
                return 0.0
                
            # 将字节数据转换为16位整数数组（立体声）
            audio_array = np.frombuffer(audio_data, dtype=np.int16)
            
            # 立体声数据，计算左右声道的平均值
            if len(audio_array) >= 2:
                # 分离左右声道
                left_channel = audio_array[::2]  # 左声道（偶数索引）
                right_channel = audio_array[1::2]  # 右声道（奇数索引）
                
                # 计算双声道的RMS音量
                rms_left = np.sqrt(np.mean(left_channel.astype(np.float32) ** 2))
                rms_right = np.sqrt(np.mean(right_channel.astype(np.float32) ** 2))
                
                # 使用两个声道的平均值
                rms = (rms_left + rms_right) / 2
                normalized_rms = rms / 32768.0
                return normalized_rms
            else:
                return 0.0
                
        except Exception as e:
            self.logger.error(f"44.1kHz立体声音量计算错误: {e}")
            return 0.0
    
    def calculate_volume_level(self, audio_data):
        """兼容旧版本的音量计算（用于16kHz单声道）"""
        return self.calculate_volume_level_44100_stereo(audio_data)
    
    def calibrate_ambient_noise(self):
        """校准环境噪音水平"""
        print("🔧 正在校准环境噪音水平...")
        self.logger.info("🔧 校准环境噪音水平...")
        try:
            # 录制3秒环境音来校准噪音水平
            print("🔇 请保持安静3秒进行环境噪音校准...")
            cmd = [
                "arecord", "-D", self.usb_mic_device,
                "-f", "S16_LE", "-r", "44100", "-c", "2",  # 44.1kHz立体声
                "-d", "3",
                "-q"
            ]
            
            result = subprocess.run(cmd, capture_output=True, timeout=6)
            
            if result.returncode == 0:
                audio_data = result.stdout
                if len(audio_data) > 0:
                    volume = self.calculate_volume_level_44100_stereo(audio_data)
                    self.ambient_noise_level = max(0.003, volume)
                    
                    # 设置唤醒阈值
                    self.wake_threshold = self.ambient_noise_level * 3
                    
                    print(f"✅ 环境噪音校准完成: {self.ambient_noise_level:.4f}")
                    print(f"✅ 唤醒阈值设置: {self.wake_threshold:.4f}")
                    self.logger.info(f"✅ 环境噪音校准完成: {self.ambient_noise_level:.4f}")
                    self.logger.info(f"✅ 唤醒阈值设置: {self.wake_threshold:.4f}")
                else:
                    print("⚠️ 录制的音频数据为空，使用默认值")
                    self.logger.warning("⚠️ 录制的音频数据为空")
            else:
                print("⚠️ 环境噪音校准失败，使用默认值")
                self.logger.warning("⚠️ 环境噪音校准失败，使用默认值")
                
        except Exception as e:
            print(f"⚠️ 环境噪音校准异常: {e}")
            self.logger.warning(f"⚠️ 环境噪音校准异常: {e}")
    
    def enter_sleep_mode(self):
        """进入休眠模式"""
        if self.sleep_mode:
            return
            
        print("\n😴 系统正在进入休眠模式...")
        self.sleep_mode = True
        self.logger.info("😴 进入休眠模式...")
        self.play_voice("sleep_mode")
        
        print("🔄 复位机械臂到安全位置...")
        # 复位机械臂到安全位置
        self.reset_arm_optimized()
        
        print("🧹 清理系统内存...")
        # 清理内存
        self._cleanup_memory()
        
        # 重置调试统计
        self.wake_attempts = 0
        self.successful_wakes = 0
        self.volume_samples = []
        
        print("💤 系统已进入休眠模式")
        print("🔊 请大声说话或拍手唤醒系统")
        print("🎵 使用44.1kHz立体声高质量监听")
        print(f"📊 当前唤醒阈值: {self.wake_threshold:.4f}")
        print(f"⏱️ 需要持续 {self.wake_duration}秒 超过阈值的声音")
        
        self.logger.info("💤 系统已休眠，使用44.1kHz立体声监听等待唤醒...")
        self.logger.info(f"🔊 当前唤醒阈值: {self.wake_threshold:.4f}")
        self.logger.info(f"⏱️ 需要持续 {self.wake_duration}秒 超过阈值的声音")
        
        # 启动实时唤醒监听
        self.wake_listener_thread = threading.Thread(target=self.realtime_wake_listener, daemon=True)
        self.wake_listener_thread.start()
        print("🎯 44.1kHz立体声实时唤醒监听线程已启动")
    
    def wake_up(self):
        """从休眠模式唤醒"""
        if not self.sleep_mode:
            return
            
        print("\n🌅 正在唤醒系统...")
        self.sleep_mode = False
        self.last_activity_time = time.time()
        self.logger.info("🌅 唤醒系统!")
        
        # 记录唤醒统计
        if self.wake_attempts > 0:
            success_rate = (self.successful_wakes / self.wake_attempts) * 100
            print(f"📊 本次休眠统计: 尝试{self.wake_attempts}次, 成功率{success_rate:.1f}%")
            self.logger.info(f"📊 本次休眠统计: 尝试{self.wake_attempts}次, 成功率{success_rate:.1f}%")
        
        self.play_voice("wake_up")
        time.sleep(1)
        self.play_voice("system_ready")
        print("✅ 系统唤醒完成，准备接收指令")

    def update_activity_time(self):
        """更新活动时间"""
        self.last_activity_time = time.time()
        if self.sleep_mode:
            self.wake_up()
    
    def set_listening_status(self, status):
        """设置监听状态"""
        self.listening = status
        if status:
            self.logger.info("👂 监听中...")
            print("👂 监听中...", end="", flush=True)
        else:
            print("")  # 换行
    
    def play_voice(self, voice_name):
        """播放预生成的语音文件"""
        voice_file = f"../voice_prompts/{voice_name}.wav"
        
        if not os.path.exists(voice_file):
            self.logger.warning(f"❌ 语音文件不存在: {voice_file}")
            print(f"❌ 语音文件不存在: {voice_file}")
            self.speak_fallback(voice_name)
            return
        
        if self.speaking:
            return
            
        def _play():
            self.speaking = True
            try:
                self.logger.info(f"🔊 播放: {voice_name}")
                print(f"🔊 播放语音: {voice_name}")
                subprocess.run([
                    "aplay", "-D", self.usb_speaker_device,
                    "-q", voice_file
                ], capture_output=True, timeout=10)
            except Exception as e:
                self.logger.error(f"❌ 播放语音错误 {voice_name}: {e}")
                print(f"❌ 播放语音错误: {e}")
                self.speak_fallback(voice_name)
            finally:
                self.speaking = False
        
        thread = threading.Thread(target=_play)
        thread.daemon = True
        thread.start()

    def speak_fallback(self, voice_name):
        """备用语音方案"""
        voice_texts = {
            "system_start": "机械臂语音控制程序已启动",
            "system_ready": "系统就绪",
            "system_exit": "进入休眠状态",
            "sleep_mode": "进入休眠模式，请持续说话或拍手唤醒",
            "wake_up": "检测到声音，唤醒系统",
            "menu_options": "请选择选项",
            "enter_voice_mode": "进入语音控制模式",
            "enter_reset_mode": "执行机械臂复位",
            "enter_status_mode": "读取机械臂状态",
            "record_start": "请说话",
            "record_end": "录音结束",
            "record_timeout": "录音超时",
            "record_failed": "录音失败",
            "no_voice_detected": "未检测到语音",
            "no_audio_data": "没有录到音频数据",
            "processing": "正在处理",
            "analyzing_command": "正在分析指令",
            "transcribing": "正在转换语音",
            "api_calling": "正在调用接口",
            "execution_start": "开始执行命令",
            "sending_command": "发送控制指令",
            "execution_progress": "指令执行中",
            "execution_halfway": "执行过半",
            "execution_complete": "动作执行完成",
            "status_updated": "状态已更新",
            "command_parse_failed": "指令解析失败",
            "command_not_understood": "无法理解命令",
            "option_not_recognized": "选项无法识别",
            "voice_not_recognized": "没有识别到语音",
            "reading_status": "读取机械臂状态",
            "listening_start": "开始监听语音输入",
            "listening_stop": "停止监听",
        }
        
        text = voice_texts.get(voice_name, "语音提示")
        self.speak_direct(text)

    def speak_direct(self, text):
        """直接语音播放（备用方案）"""
        if self.speaking:
            return
            
        def _speak():
            self.speaking = True
            try:
                self.logger.info(f"🔊 {text}")
                print(f"🔊 语音播报: {text}")
                subprocess.run([
                    "espeak", "-v", "zh", "-s", "150", text
                ], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=5)
            except Exception as e:
                self.logger.error(f"❌ 语音播放错误: {e}")
                print(f"❌ 语音播放错误: {e}")
            finally:
                self.speaking = False
        
        thread = threading.Thread(target=_speak)
        thread.daemon = True
        thread.start()

    def wait_for_speech_completion(self):
        """等待语音播报完成"""
        max_wait = 10
        start_time = time.time()
        while self.speaking and (time.time() - start_time) < max_wait:
            time.sleep(0.1)

    def detect_voice_activity(self, audio_data, sample_rate=44100, channels=2):
        """简单的语音活动检测"""
        try:
            import numpy as np
            audio_array = np.frombuffer(audio_data, dtype=np.int16)
            
            if channels == 2:
                audio_array = audio_array.reshape(-1, 2)
                audio_array = audio_array.mean(axis=1).astype(np.int16)
            
            rms = np.sqrt(np.mean(audio_array.astype(np.float32) ** 2))
            normalized_rms = rms / 32768.0
            is_speech = normalized_rms > self.voice_threshold
            
            return is_speech, normalized_rms
            
        except Exception as e:
            self.logger.error(f"语音检测错误: {e}")
            return False, 0.0

    def remove_silence_from_audio(self, input_file, output_file):
        """去除音频中的静音部分"""
        try:
            result = subprocess.run([
                "sox", input_file, output_file,
                "silence", "-l", "1", "0.1", "1%", "-1", "0.1", "1%"
            ], capture_output=True, timeout=10)
            
            if result.returncode == 0 and os.path.exists(output_file):
                original_size = os.path.getsize(input_file)
                processed_size = os.path.getsize(output_file)
                self.logger.info(f"✅ 静音去除完成: {original_size//1024}KB → {processed_size//1024}KB")
                
                if processed_size < 10000:
                    self.logger.warning("⚠️ 文件过小，使用原始录音")
                    subprocess.run(["cp", input_file, output_file])
                    return True
                return True
            else:
                self.logger.warning("❌ 静音去除失败，使用原始文件")
                subprocess.run(["cp", input_file, output_file])
                return False
                
        except Exception as e:
            self.logger.error(f"静音去除错误: {e}")
            try:
                subprocess.run(["cp", input_file, output_file])
                return True
            except:
                return False

    def record_voice_activated(self, max_duration=8):
        """语音激活录音，自动去除静音"""
        if self.sleep_mode:
            print("💤 休眠模式下，使用唤醒监听进行录音...")
            return None
            
        self.wait_for_speech_completion()
        self.play_voice("record_start")
        time.sleep(1)
        
        try:
            print("🎤 启动语音激活录音...")
            cmd = [
                "arecord", "-D", self.usb_mic_device,
                "-f", "S16_LE", "-r", "44100", "-c", "2",
                "-t", "raw"
            ]
            
            process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
            
            audio_chunks = []
            voice_detected = False
            silence_start_time = None
            recording_start_time = time.time()
            last_voice_time = recording_start_time
            
            self.set_listening_status(True)
            self.logger.info("🔊 监听中...")
            print("🔊 正在监听语音输入...")
            
            while True:
                chunk = process.stdout.read(4096)
                if not chunk:
                    break
                
                audio_chunks.append(chunk)
                current_time = time.time()
                
                has_voice, volume = self.detect_voice_activity(chunk)
                
                if has_voice:
                    if not voice_detected:
                        voice_detected = True
                        self.logger.info("🎯 检测到语音!")
                        print(f"\n🎯 检测到语音!", end="", flush=True)
                    
                    last_voice_time = current_time
                    silence_start_time = None
                    print("💬", end="", flush=True)
                else:
                    if voice_detected:
                        if silence_start_time is None:
                            silence_start_time = current_time
                            print("\n⏳ 静音倒计时: 2", end="", flush=True)
                        else:
                            silence_duration = current_time - silence_start_time
                            countdown = max(0, 2 - int(silence_duration))
                            if countdown < 2:
                                print(f"\r⏳ 静音倒计时: {countdown}", end="", flush=True)
                            
                            if silence_duration >= self.silence_timeout:
                                self.logger.info("✅ 录音结束")
                                print(f"\n✅ 录音结束 - 检测到静音")
                                break
                    else:
                        wait_time = current_time - recording_start_time
                        if wait_time % 1 < 0.1:
                            noise_level = "⚡" if volume > 0.01 else "🔇"
                            print(noise_level, end="", flush=True)
                
                if current_time - recording_start_time >= self.max_record_time:
                    self.logger.warning("⚠️ 达到最大录音时间")
                    print(f"\n⚠️ 达到最大录音时间")
                    break
            
            process.terminate()
            process.wait()
            
            self.set_listening_status(False)
            self.wait_for_speech_completion()
            
            if not voice_detected:
                self.logger.warning("❌ 未检测到语音")
                print("\n❌ 未检测到语音")
                self.play_voice("no_voice_detected")
                return None
            
            if not audio_chunks:
                self.logger.warning("❌ 没有录到音频数据")
                print("\n❌ 没有录到音频数据")
                self.play_voice("no_audio_data")
                return None
            
            try:
                print("🔄 处理录音文件...")
                with tempfile.NamedTemporaryFile(suffix='.wav', delete=False) as temp_file:
                    raw_filename = temp_file.name
                
                sox_process = subprocess.Popen([
                    "sox", "-t", "raw", "-r", "44100", "-e", "signed", "-b", "16", "-c", "2", "-",
                    "-t", "wav", raw_filename
                ], stdin=subprocess.PIPE, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
                
                for chunk in audio_chunks:
                    sox_process.stdin.write(chunk)
                
                sox_process.stdin.close()
                sox_process.wait()
                
                if sox_process.returncode == 0 and os.path.exists(raw_filename):
                    with tempfile.NamedTemporaryFile(suffix='.wav', delete=False) as temp_file:
                        processed_filename = temp_file.name
                    
                    print("🔇 去除静音部分...")
                    if self.remove_silence_from_audio(raw_filename, processed_filename):
                        os.unlink(raw_filename)
                        file_size = os.path.getsize(processed_filename)
                        self.logger.info(f"✅ 处理后文件大小: {file_size//1024}KB")
                        print(f"✅ 录音处理完成，文件大小: {file_size//1024}KB")
                        self.play_voice("record_end")
                        return processed_filename
                    else:
                        file_size = os.path.getsize(raw_filename)
                        self.logger.info(f"✅ 原始录音文件大小: {file_size//1024}KB")
                        print(f"✅ 使用原始录音，文件大小: {file_size//1024}KB")
                        self.play_voice("record_end")
                        return raw_filename
                else:
                    self.play_voice("record_failed")
                    return None
                    
            except Exception as e:
                self.logger.error(f"文件处理错误: {e}")
                print(f"❌ 文件处理错误: {e}")
                self.play_voice("record_failed")
                return None
                
        except Exception as e:
            self.logger.error(f"录音错误: {e}")
            print(f"❌ 录音错误: {e}")
            self.set_listening_status(False)
            self.play_voice("record_failed")
            return None

    def record_audio_fallback(self, duration=5):
        """备用录音方法"""
        if self.sleep_mode:
            print("💤 休眠模式下，使用唤醒监听进行录音...")
            return None
            
        self.wait_for_speech_completion()
        self.play_voice("record_start")
        time.sleep(1)
        
        self.logger.info(f"🎤 备用录音 {duration}秒...")
        print(f"🎤 备用录音 {duration}秒...")
        
        try:
            with tempfile.NamedTemporaryFile(suffix='.wav', delete=False) as temp_file:
                temp_filename = temp_file.name
            
            self.set_listening_status(True)
            
            result = subprocess.run([
                "arecord", "-D", self.usb_mic_device,
                "-f", "S16_LE", "-r", "44100", "-c", "2",
                "-d", str(duration), temp_filename
            ], capture_output=True, timeout=duration+2)
            
            self.set_listening_status(False)
            self.wait_for_speech_completion()
            
            if result.returncode == 0 and os.path.exists(temp_filename):
                with tempfile.NamedTemporaryFile(suffix='.wav', delete=False) as processed_file:
                    processed_filename = processed_file.name
                
                if self.remove_silence_from_audio(temp_filename, processed_filename):
                    os.unlink(temp_filename)
                    file_size = os.path.getsize(processed_filename)
                    self.logger.info(f"✅ 备用录音完成, 大小: {file_size//1024}KB")
                    print(f"✅ 备用录音完成, 大小: {file_size//1024}KB")
                    self.play_voice("record_end")
                    return processed_filename
                else:
                    file_size = os.path.getsize(temp_filename)
                    self.logger.info(f"✅ 备用录音完成, 大小: {file_size//1024}KB (未处理)")
                    print(f"✅ 备用录音完成, 大小: {file_size//1024}KB (未处理)")
                    self.play_voice("record_end")
                    return temp_filename
            else:
                self.play_voice("record_failed")
                return None
                
        except Exception as e:
            self.logger.error(f"备用录音错误: {e}")
            self.set_listening_status(False)
            self.play_voice("record_failed")
            return None

    def convert_to_mono(self, stereo_file):
        """将立体声转换为单声道"""
        if not stereo_file or not os.path.exists(stereo_file):
            return None
            
        try:
            mono_file = stereo_file + "_mono.wav"
            result = subprocess.run([
                "sox", stereo_file, "-r", "16000", "-c", "1", mono_file
            ], capture_output=True, timeout=5)
            
            if result.returncode == 0 and os.path.exists(mono_file):
                os.unlink(stereo_file)
                return mono_file
            else:
                return stereo_file
                
        except Exception as e:
            self.logger.error(f"音频转换错误: {e}")
            return stereo_file

    def transcribe_audio_optimized(self, audio_file):
        """优化转录流程"""
        if not audio_file or not os.path.exists(audio_file):
            return ""
            
        start_time = time.time()
        self.wait_for_speech_completion()
        self.play_voice("transcribing")
        print("🔤 正在转换语音为文字...")
            
        try:
            file_size = os.path.getsize(audio_file)
            if file_size < 50000:
                self.logger.warning("❌ 录音文件过小")
                print("❌ 录音文件过小，可能没有有效语音")
                os.unlink(audio_file)
                return ""
            
            print("🔄 转换为单声道音频...")
            mono_file = self.convert_to_mono(audio_file)
            if not mono_file:
                return ""
            
            if not self.recognizer:
                self.recognizer = KaldiRecognizer(self.model, 16000)
            
            text_result = ""
            with open(mono_file, 'rb') as f:
                audio_data = f.read()
            
            print("🎯 进行语音识别...")
            chunk_size = 8000
            for i in range(0, len(audio_data), chunk_size):
                chunk = audio_data[i:i+chunk_size]
                if self.recognizer.AcceptWaveform(chunk):
                    result = json.loads(self.recognizer.Result())
                    if result['text']:
                        text_result += result['text'] + " "
            
            final_result = json.loads(self.recognizer.FinalResult())
            if final_result['text']:
                text_result += final_result['text']
            
            if os.path.exists(mono_file):
                os.unlink(mono_file)
            
            transcribe_time = time.time() - start_time
            self.logger.info(f"✅ 转录完成: {transcribe_time:.1f}s, 文本: {text_result.strip()}")
            print(f"✅ 语音识别完成: {transcribe_time:.1f}秒")
            print(f"📝 识别结果: {text_result.strip()}")
            
            return text_result.strip()
            
        except Exception as e:
            self.logger.error(f"转录错误: {e}")
            print(f"❌ 语音识别错误: {e}")
            for file_path in [audio_file, audio_file + "_mono.wav"]:
                if os.path.exists(file_path):
                    os.unlink(file_path)
            return ""

    def analyze_voice_command_with_timeout(self, text, timeout=20):
        """带超时的API调用"""
        if not text.strip():
            return {"error": "空指令"}
        
        cache_key = text.strip()
        if cache_key in self.command_cache:
            self.logger.info("💾 使用缓存命令")
            print("💾 使用缓存命令解析结果")
            return self.command_cache[cache_key]
        
        self.wait_for_speech_completion()
        self.play_voice("api_calling")
        print("🌐 正在调用AI接口解析指令...")
            
        def _api_call():
            return self._call_voice_api(text)
        
        try:
            self.logger.info(f"🔄 API调用 (超时: {timeout}s)...")
            print(f"🔄 API调用中 (超时: {timeout}秒)...")
            start_time = time.time()
            
            future = self.api_executor.submit(_api_call)
            result = future.result(timeout=timeout)
            
            api_time = time.time() - start_time
            self.logger.info(f"✅ API响应: {api_time:.1f}s")
            print(f"✅ API响应完成: {api_time:.1f}秒")
            
            if "error" not in result:
                if len(self.command_cache) >= self.cache_size:
                    self.command_cache.pop(next(iter(self.command_cache)))
                self.command_cache[cache_key] = result
                self.last_api_success = True
                print("✅ 指令解析成功")
            else:
                self.last_api_success = False
                print("❌ 指令解析失败")
                
            return result
            
        except TimeoutError:
            self.logger.error("❌ API调用超时")
            print("❌ API调用超时")
            self.last_api_success = False
            return {"error": "API调用超时"}
        except Exception as e:
            self.logger.error(f"❌ API调用异常: {e}")
            print(f"❌ API调用异常: {e}")
            self.last_api_success = False
            return {"error": f"API调用异常: {e}"}

    def _call_voice_api(self, text):
        """实际的API调用"""
        system_prompt = """你是一个机械臂控制专家。请将用户的语音命令转换为具体的舵机控制指令。

机械臂有6个舵机：
1. base (底座): 0-1000位置，控制整体左右旋转
2. shoulder (肩部): 0-1000位置，控制大臂上下俯仰
3. elbow (肘部): 0-1000位置，控制小臂前后伸展
4. wrist_vert (腕部垂直): 0-1000位置，控制手腕上下摆动
5. wrist_rot (腕部旋转): 0-1000位置，控制手腕水平旋转
6. gripper (夹爪): 0-1000位置，0为闭合，1000为完全打开

请根据用户指令，生成JSON格式的控制指令，包含以下字段：
- action: 动作描述
- commands: 舵机控制列表，每个命令包含 servo(舵机名称) 和 position(目标位置，0-1000)
- duration: 执行时间(秒)

请只返回JSON格式，不要其他文字。"""

        headers = {
            "Authorization": f"Bearer {self.api_key}",
            "Content-Type": "application/json"
        }
        
        data = {
            "model": self.model_name,
            "messages": [
                {"role": "system", "content": system_prompt},
                {"role": "user", "content": f"当前状态: {self.arm_state}。指令: {text}"}
            ],
            "temperature": 0.3,
            "max_tokens": 500
        }
        
        try:
            response = requests.post(
                f"{self.base_url}/chat/completions",
                headers=headers,
                json=data,
                timeout=25
            )
            
            if response.status_code == 200:
                result = response.json()
                response_text = result["choices"][0]["message"]["content"].strip()
                
                try:
                    return json.loads(response_text)
                except:
                    json_match = re.search(r'\{.*\}', response_text, re.DOTALL)
                    if json_match:
                        return json.loads(json_match.group())
                    else:
                        return {"error": "无法解析AI响应"}
            else:
                return {"error": f"API调用失败: {response.status_code}"}
                
        except Exception as e:
            return {"error": f"API调用异常: {e}"}

    def interactive_control_optimized(self):
        """优化交互控制"""
        self.play_voice("system_ready")
        
        while self.running:
            if self.sleep_mode:
                time.sleep(1)
                continue
                
            self._cleanup_memory()
            
            menu_options = {
                "1": "语音控制",
                "2": "复位机械臂", 
                "3": "读取状态",
                "4": "进入休眠"
            }
            
            print("\n" + "="*50)
            print("🤖 机械臂语音控制系统")
            print(f"⏰ 空闲时间: {(time.time() - self.last_activity_time):.1f}秒")
            print("="*50)
            
            print("\n📋 可用选项:")
            for key, description in menu_options.items():
                print(f"  {key}. {description}")
            
            self.play_voice("menu_options")
            print("\n💡 请说出您的选择 (1-4):")
            
            audio_file = self.record_voice_activated()
            if not audio_file:
                self.play_voice("record_failed")
                print("🔄 尝试备用录音方式...")
                audio_file = self.record_audio_fallback(duration=4)
            
            if audio_file:
                command = self.transcribe_audio_optimized(audio_file)
                self.logger.info(f"📝 识别结果: {command}")
                print(f"📝 语音识别结果: {command}")
                
                if command:
                    self.update_activity_time()
                    
                    if "1" in command or "一" in command or "语音" in command:
                        print("🎤 进入语音控制模式")
                        self.play_voice("enter_voice_mode")
                        self.voice_control_mode_optimized()
                    elif "2" in command or "二" in command or "复位" in command:
                        print("🔄 执行机械臂复位")
                        self.play_voice("enter_reset_mode")
                        self.reset_arm_optimized()
                    elif "3" in command or "三" in command or "状态" in command:
                        print("📊 读取机械臂状态")
                        self.play_voice("enter_status_mode")
                        self.read_servo_status_optimized()
                    elif "4" in command or "四" in command or "休眠" in command or "退出" in command:
                        print("😴 进入休眠模式")
                        self.play_voice("system_exit")
                        self.enter_sleep_mode()
                    else:
                        self.play_voice("option_not_recognized")
                        self.logger.warning("❌ 无法识别选项")
                        print("❌ 无法识别选项，请重试")
                else:
                    self.play_voice("voice_not_recognized")
                    self.logger.warning("❌ 没有识别到语音")
                    print("❌ 没有识别到语音，请重试")
            else:
                self.play_voice("record_failed")
                self.logger.warning("❌ 录音失败")
                print("❌ 录音失败，请检查麦克风")

    def voice_control_mode_optimized(self):
        """优化语音控制模式"""
        if self.sleep_mode:
            return
            
        self.play_voice("enter_voice_mode")
        self.logger.info("🎤 进入语音控制模式")
        print("\n🎤 进入语音控制模式")
        self.play_voice("listening_start")
        
        total_start_time = time.time()
        
        audio_file = self.record_voice_activated()
        if not audio_file:
            self.play_voice("record_failed")
            self.logger.info("🔄 尝试备用录音...")
            print("🔄 尝试备用录音...")
            audio_file = self.record_audio_fallback(duration=5)
        
        if not audio_file:
            self.play_voice("record_failed")
            return
        
        command = self.transcribe_audio_optimized(audio_file)
        if not command:
            self.play_voice("voice_not_recognized")
            return
        
        self.speak_direct(f"收到指令：{command}")
        self.logger.info(f"🗣️ 识别指令: {command}")
        print(f"🗣️ 识别指令: {command}")
        
        self.play_voice("analyzing_command")
        command_data = self.analyze_voice_command_with_timeout(command)
        
        if "error" in command_data:
            error_msg = command_data["error"]
            self.play_voice("command_parse_failed")
            self.logger.error(f"❌ 指令解析失败: {error_msg}")
            print(f"❌ 指令解析失败: {error_msg}")
            return
        
        action = command_data.get('action', '未知动作')
        duration = command_data.get('duration', 2)
        
        self.speak_direct(f"解析到动作：{action}")
        self.logger.info(f"🔍 解析动作: {action}, 时长: {duration}秒")
        print(f"🔍 解析动作: {action}, 时长: {duration}秒")
        
        total_time = time.time() - total_start_time
        self.speak_direct(f"开始执行，处理时间{total_time:.1f}秒")
        self.logger.info(f"⏱️ 总处理时间: {total_time:.1f}秒")
        print(f"⏱️ 总处理时间: {total_time:.1f}秒")
        self.execute_commands_optimized(command_data)

    def read_servo_status_optimized(self):
        """优化状态读取"""
        if self.sleep_mode:
            return
            
        self.play_voice("enter_status_mode")
        self.logger.info("📊 读取舵机状态")
        print("\n📊 读取舵机状态")
        status_text = "舵机状态: "
        for servo_name, position in self.arm_state.items():
            status_text += f"{servo_name}位置{position}, "
        
        self.speak_direct(status_text)
        self.logger.info(f"📊 {status_text}")
        print(f"📊 {status_text}")

def main():
    controller = None
    try:
        print("="*60)
        print("🎉 欢迎使用机械臂语音控制系统")
        print("="*60)
        controller = RoboticArmController()
        
        print("\n🚀 系统启动完成，开始主循环...")
        print("💡 按 Ctrl+C 退出程序")
        
        while controller.running:
            controller.interactive_control_optimized()
            time.sleep(0.1)
            
    except KeyboardInterrupt:
        if controller:
            print("\n\n👋 用户中断，正在安全关闭系统...")
            controller.logger.info("👋 用户中断，关闭程序")
    except Exception as e:
        if controller:
            controller.logger.error(f"❌ 程序运行错误: {e}")
            print(f"\n❌ 系统错误: {e}")
        else:
            print(f"❌ 程序启动错误: {e}")
    finally:
        if controller:
            print("\n🔚 系统关闭中...")
            controller.cleanup()
        print("👋 感谢使用机械臂语音控制系统！")

if __name__ == "__main__":
    main()